L1=Link('d',0.089159,'a',0,'alpha',pi/2,'standard');
L2=Link('d',0,'a',-0.425,'alpha',0,'offset',-pi/2,'standard');
L3=Link('d',0,'a',-0.39225,'alpha',0,'standard');
L4=Link('d',0.10915,'a',0,'alpha',pi/2,'offset',-pi/2,'standard');
L5=Link('d',0.09465,'a',0,'alpha',-pi/2,'standard');
L6=Link('d',0.0823,'a',0,'alpha',0,'standard');
UR5=SerialLink([L1,L2,L3,L4,L5,L6],'name','UR5');
UR5.teach
UR5.display();
%%
% ------MDH
L1=Link('d',0.147,'a',0,'alpha',0,'modified');
L2=Link('d',0,'a',0,'alpha',pi/2,'offset',-pi/2,'modified');
L3=Link('d',0,'a',-0.427,'alpha',0,'modified');
L4=Link('d',0.141,'a',-0.357,'alpha',0,'offset',-pi/2,'modified');
L5=Link('d',0.116,'a',0,'alpha',pi/2,'modified');
L6=Link('d',0.105,'a',0,'alpha',-pi/2,'modified');
CR5=SerialLink([L1,L2,L3,L4,L5,L6],'name','CR5');
forword=CR5.fkine([-86.907,-17.988,-76.515,6.4682,96.532,-13.719]*pi/180);
CR5.plot([-86.907,-17.988,-76.515,6.4682,96.532,-13.719]*pi/180)
disp(forword)
CR5.teach()
CR5.display();
%%
%--------SDH
L1=Link('d',0.147,'a',0,'alpha',pi/2,'standard');
L2=Link('d',0,'a',-0.4270,'alpha',0,'offset',-pi/2,'standard');
L3=Link('d',0,'a',-0.357,'alpha',0,'standard');
L4=Link('d',0.141,'a',0,'alpha',pi/2,'offset',-pi/2,'standard');
L5=Link('d',0.116,'a',0,'alpha',-pi/2,'standard');
L6=Link('d',0.105,'a',0,'alpha',0,'standard');
CR5=SerialLink([L1,L2,L3,L4,L5,L6],'name','CR5');
forword=CR5.fkine([-86.907,-17.988,-76.515,6.4682,96.532,-13.719]*pi/180);
CR5.plot([-86.907,-17.988,-76.515,6.4682,96.532,-13.719]*pi/180)
disp(forword)
weizi=transl(-0.396,-0.380,0.300);
ikne=CR5.ikunc(weizi);
CR5.plot([-1.6906482959688576,-0.05058863016844344,-1.1737779915297994,-0.324266920842884,1.6061472552090483,0]);
CR5.teach()
%%
clc;close all;clear;
load("4.20calibration.mat")
%load("Intercalibration.mat")
size_pic=calibrationSession.CameraParameters.ImageSize;
writematrix(size_pic,'size_pic.txt')
cam_rotation=calibrationSession.CameraParameters.RotationVectors;
writematrix(cam_rotation,'cam_rotation.txt')
cam_tran=calibrationSession.CameraParameters.TranslationVectors;
writematrix(cam_tran,'cam_tran.txt')
cam_radia=calibrationSession.CameraParameters.RadialDistortion;
writematrix(cam_radia,'cam_radia.txt')
cam_tan=calibrationSession.CameraParameters.TangentialDistortion;
writematrix(cam_tan,'cam_tan.txt')
cam_intri=calibrationSession.CameraParameters.IntrinsicMatrix;
writematrix(cam_intri,'cam_intri.txt')